#ifndef SOPHUS_VELOCITIES_HPP
#define SOPHUS_VELOCITIES_HPP

#include <functional>

#include "num_diff.hpp"
#include "se3.hpp"

namespace Sophus
{
  namespace experimental
  {
    // Experimental since the API will certainly change drastically in the future.

    // Transforms velocity vector by rotation ``foo_R_bar``.
    //
    // Note: vel_bar can be either a linear or a rotational velocity vector.
    //
    template <class Scalar>
    Vector3<Scalar> transformVelocity(SO3<Scalar> const &foo_R_bar,
                                      Vector3<Scalar> const &vel_bar)
    {
      // For rotational velocities note that:
      //
      //   vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T)
      //           = vee(hat(Adj(foo_R_bar) * vel_bar))
      //           = Adj(foo_R_bar) * vel_bar
      //           = foo_R_bar * vel_bar.
      //
      return foo_R_bar * vel_bar;
    }

    // Transforms velocity vector by pose ``foo_T_bar``.
    //
    // Note: vel_bar can be either a linear or a rotational velocity vector.
    //
    template <class Scalar>
    Vector3<Scalar> transformVelocity(SE3<Scalar> const &foo_T_bar,
                                      Vector3<Scalar> const &vel_bar)
    {
      return transformVelocity(foo_T_bar.so3(), vel_bar);
    }

    // finite difference approximation of instantanious velocity in frame foo
    //
    template <class Scalar>
    Vector3<Scalar> finiteDifferenceRotationalVelocity(
        std::function<SO3<Scalar>(Scalar)> const &foo_R_bar, Scalar t,
        Scalar h = Constants<Scalar>::epsilon())
    {
      // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor
      //
      // W = dR(t)/dt * R^{-1}(t)
      Matrix3<Scalar> dR_dt_in_frame_foo = curveNumDiff(
          [&foo_R_bar](Scalar t0) -> Matrix3<Scalar> {
            return foo_R_bar(t0).matrix();
          },
          t, h);
      // velocity tensor
      Matrix3<Scalar> W_in_frame_foo =
          dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix();
      return SO3<Scalar>::vee(W_in_frame_foo);
    }

    // finite difference approximation of instantanious velocity in frame foo
    //
    template <class Scalar>
    Vector3<Scalar> finiteDifferenceRotationalVelocity(
        std::function<SE3<Scalar>(Scalar)> const &foo_T_bar, Scalar t,
        Scalar h = Constants<Scalar>::epsilon())
    {
      return finiteDifferenceRotationalVelocity<Scalar>(
          [&foo_T_bar](Scalar t) -> SO3<Scalar> { return foo_T_bar(t).so3(); }, t,
          h);
    }

  } // namespace experimental
} // namespace Sophus

#endif // SOPHUS_VELOCITIES_HPP
